package modes;

import main.ColorSensorChew;
import main.Engine;
import main.SensorEngine;
import main.SonicSensor;
import navigation.Direction;
import navigation.IllegalMoveException;
import navigation.Navigator;

/**
 * ModeFoundObject will be used, if an object has been found
 * @author Max
 */
public class ModeFoundObject extends AMode
{

    private int countWhite = 0;
    private int count = 0;
    private final int MAX_COUNT_WHITE = 40;
    private int currentMaxWhite = MAX_COUNT_WHITE / 2;
    private int failureCount = 0;
    private boolean moving = false;
    private boolean turn = false;
    private boolean Objectgone = false;

    /**
     *Creates an object of ModeFoundObject
     * @param en Engine
     * @param se SensorEngine
     * @param cs SoclorSensor
     * @param ss SonicSensor
     * @param navi Navigator
     */
    public ModeFoundObject(Engine en, SensorEngine se, ColorSensorChew cs, SonicSensor ss, Navigator navi)
    {
        super(en, se, cs, ss, navi);

    }

    /**
     * Checks if object moved.
     * Chewie drives close to the object and turns around.
     * Sets new direction.
     * If object didn't move, he informs the navigator that there is a object

     * @return new Mode
     * @throws InterruptedException
     */
    public Mode run()
            throws InterruptedException
    {

        Mode mode = Mode.FOUND_OBJECT;
        ModeController.dm.echo("Found-Object");
        engine.stop();
        rotationSensor.rotateMiddle();
        Thread.sleep(150);
        mode = Mode.BLACK;

        if (!moving)
        {
            isMoving();
        }
        mode = driveNearToObject();
        if (sonic.isObject() && !moving && turn)
        {
            addField();
            moving = false;
            ModeController.dm.echo("add Field");
            turn = false;
            mode = Mode.BLACK;

        } else if (sonic.isObject() && moving && turn)
        {
            moving = false;
            turn = false;
            ModeController.dm.echo("Found-Moving");
            driveBack();
            mode = Mode.BLACK;
        }

        Objectgone = false;
        return mode;
    }

    private void turn() throws InterruptedException
    {
        stepToObject();
        engine.rotate180();
        rotationSensor.rotateMiddle();
        engine.setRotateRight(false);
        engine.backward();
        Thread.sleep(1300);
    }

    private boolean isMoving() throws InterruptedException
    {

        sonic.calcDistance();
        int start = sonic.getCurrentValue();
        for (int i = 0; i < 75; ++i)
        {
            if (!sonic.isObject())
            {

                break;
            } else if (Math.abs(sonic.getCurrentValue() - start) > 2)
            {
                moving = true;
                break;
            }
            Thread.sleep(50);
        }
        return moving;
    }

    private void addField() throws InterruptedException
    {
        try
        {
            navi.objectFound();
            navi.moveOneStep();
            turn();
            navi.directionChanged(Direction.BACKWARD);
        } catch (IllegalMoveException ex)
        {
            ModeController.dm.echo(ex.toString());
        }
    }

    private Mode driveNearToObject() throws InterruptedException
    {
        engine.forward();
        engine.setSpeed(100);
        Mode mode = Mode.BLACK.BLACK;
        do
        {
            count++;

            mode = followLine();

            if (isObjectOrClose())
            {
                engine.stop();
                break;
            }

            if (mode != Mode.BLACK && moving)
            {
                mode = Mode.BLACK;
            }

        } while (mode == Mode.BLACK);
        resetBlack();
        return mode;
    }

    private void driveBack() throws InterruptedException
    {
        navi.directionChanged(Direction.BACKWARD);
        engine.backward();
        engine.setSpeed(350);
        Thread.sleep(1200);
        turn();
        engine.backward();
        engine.setSpeed(350);
        Thread.sleep(1100);
    }

    private Mode followLine() throws InterruptedException
    {
        Mode mode = Mode.BLACK;
        rotationSensor.rotateColorSensor();
        color.readColor();

        if (color.isColorBlack())
        {
            actionBlack();
        } else if (color.isColorWhite())
        {
            actionWhite();
        } else if (color.isColorBlue())
        {
            actionBlack();
            mode = Mode.BLUE;
        } else if (color.isColorGreen())
        {
            actionBlack();
            mode = Mode.GREEN;
        } else if (color.isColorRed())
        {
            actionBlack();
            mode = Mode.RED;
        } else if (color.isColorYellow())
        {
            actionBlack();
            mode = Mode.YELLOW;
        }
        if (failureCount > 4)
        {
            int ex = 0;
            if (engine.isRotateRight())
            {
                ex = 30;
            }
            for (int i = 0; i < (currentMaxWhite) / 2 + ex; i++)
            {
                color.readColor();
                if (color.isColorBlack())
                {
                    engine.stop();
                }
            }

            engine.stop();
            mode = Mode.YELLOW_WITHOUT_CHECK;
        }

        return mode;
    }

    private void actionWhite() throws InterruptedException
    {
        engine.setSpeed(350);
        if (rotationSensor.isMiddle())
        {
            rotationSensor.setRotating(false);
            if (countWhite == 0)
            {
                if (failureCount == 0)
                {
                }
                engine.switchRotation();
                failureCount++;

            } else if (countWhite >= currentMaxWhite)
            {
                engine.switchRotation();
                countWhite = 0;
                failureCount++;
                failureMode();

            }
            countWhite++;
        } else if (rotationSensor.isLeft())
        {
            engine.driveRight(70);

        } else if (rotationSensor.isRight())
        {
            engine.driveLeft(70);

        }
    }

    private void actionBlack()
    {
        rotationSensor.setRotating(true);
        resetBlack();

        if (rotationSensor.isLeft())
        {
            engine.driveLeft(70);

        } else if (rotationSensor.isRight())
        {
            engine.driveRight(70);

        } else
        {
            if (!sonic.isObject())
            {
                Objectgone = true;
            }
            engine.forward();

        }
        engine.setSpeed(100);
    }

    private void resetBlack()
    {
        failureCount = 0;
        countWhite = 0;
        currentMaxWhite = MAX_COUNT_WHITE / 2;
    }

    private void failureMode()
    {
        if (failureCount > 2)
        {
            currentMaxWhite += 15;
        } else
        {
            currentMaxWhite = MAX_COUNT_WHITE;
        }
    }

    /**
     * Tells the mode wether the object is moving or not
     * @param moving
     */
    public void setMoving(boolean moving)
    {
        this.moving = moving;
    }

    private void findLine()
    {
        int maxcount = 100;
        int countI = 0;
        do
        {
            countI++;
            color.readColor();
            if (countI >= maxcount)
            {
                engine.switchRotation();
                maxcount += 100;
                countI = 0;
            }
        } while (!color.isColorWhite());
    }

    private void stepToObject() throws InterruptedException
    {
        rotationSensor.rotate(90);
        engine.forward();
        Thread.sleep(600);
    }

    private boolean isObjectOrClose()
    {
        if (sonic.getCurrentValue() <= 6 || Objectgone)
        {
            if (sonic.getCurrentValue() <= 6)
            {
                turn = true;
            } else
            {
                moving = false;
            }
            return true;
        }
        return false;
    }
}
